/*
Copyright (C) 1996-1997 Id Software, Inc.

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

*/
// mathlib.c -- math primitives

#include <math.h>
#if __SSE__
#include <xmmintrin.h>
#endif
#include "quakedef.h"

vec3_t vec3_origin = {0,0,0};

/*-----------------------------------------------------------------*/

void ProjectVector(const vec3_t b, const vec3_t a, vec3_t c)
{
    float dpa,dpab;

    dpa = DotProduct(a, a);
    dpab = DotProduct(a, b) / dpa;

    c[0] = a[0] * dpab;
    c[1] = a[1] * dpab;
    c[2] = a[2] * dpab;
}

void ProjectPlane(const vec3_t src,const vec3_t v1,const vec3_t v2,vec3_t dst)
{
    vec3_t t1,t2;

    ProjectVector(src, v1, t1);
    ProjectVector(src, v2, t2);

    VectorAdd(t1,t2,dst);
}

void VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
{
#if __SSE__
    // raynorpat: sse optimization
    __m128 xmm_veca, xmm_vecb, xmm_out;

    xmm_veca = _mm_load_ss(&veca[0]);
    xmm_vecb = _mm_load_ss(&vecb[0]);
    xmm_out = _mm_sub_ss(xmm_veca, xmm_vecb);
    _mm_store_ss(&out[0], xmm_out);

    xmm_veca = _mm_load_ss(&veca[1]);
    xmm_vecb = _mm_load_ss(&vecb[1]);
    xmm_out = _mm_sub_ss(xmm_veca, xmm_vecb);
    _mm_store_ss(&out[1], xmm_out);

    xmm_veca = _mm_load_ss(&veca[2]);
    xmm_vecb = _mm_load_ss(&vecb[2]);
    xmm_out = _mm_sub_ss(xmm_veca, xmm_vecb);
    _mm_store_ss(&out[2], xmm_out);
#else
    out[0] = veca[0]-vecb[0];
    out[1] = veca[1]-vecb[1];
    out[2] = veca[2]-vecb[2];
#endif
}

void VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
{
#if __SSE__
    // raynorpat: sse optimization
    __m128 xmm_veca, xmm_vecb, xmm_out;

    xmm_veca = _mm_load_ss(&veca[0]);
    xmm_vecb = _mm_load_ss(&vecb[0]);
    xmm_out = _mm_add_ss(xmm_veca, xmm_vecb);
    _mm_store_ss(&out[0], xmm_out);

    xmm_veca = _mm_load_ss(&veca[1]);
    xmm_vecb = _mm_load_ss(&vecb[1]);
    xmm_out = _mm_add_ss(xmm_veca, xmm_vecb);
    _mm_store_ss(&out[1], xmm_out);

    xmm_veca = _mm_load_ss(&veca[2]);
    xmm_vecb = _mm_load_ss(&vecb[2]);
    xmm_out = _mm_add_ss(xmm_veca, xmm_vecb);
    _mm_store_ss(&out[2], xmm_out);
#else
    out[0] = veca[0]+vecb[0];
    out[1] = veca[1]+vecb[1];
    out[2] = veca[2]+vecb[2];
#endif
}

void VectorCopy (vec3_t in, vec3_t out)
{
#if __SSE__
    // raynorpat: sse optimization
    __m128 xmm_in;

    xmm_in = _mm_load_ss(&in[0]);
    _mm_store_ss(&out[0], xmm_in);

    xmm_in = _mm_load_ss(&in[1]);
    _mm_store_ss(&out[1], xmm_in);

    xmm_in = _mm_load_ss(&in[2]);
    _mm_store_ss(&out[2], xmm_in);
#else
    out[0] = in[0];
    out[1] = in[1];
    out[2] = in[2];
#endif
}

void VectorScale (vec3_t in, vec_t scale, vec3_t out)
{
#if __SSE__
    // raynorpat: sse optimization
    __m128 xmm_in, xmm_scale, xmm_out;

    xmm_in = _mm_load_ss(&in[0]);
    xmm_scale = _mm_load_ss(&scale);
    xmm_out = _mm_mul_ss(xmm_in, xmm_scale);
    _mm_store_ss(&out[0], xmm_out);

    xmm_in = _mm_load_ss(&in[1]);
    xmm_scale = _mm_load_ss(&scale);
    xmm_out = _mm_mul_ss(xmm_in, xmm_scale);
    _mm_store_ss(&out[1], xmm_out);

    xmm_in = _mm_load_ss(&in[2]);
    xmm_scale = _mm_load_ss(&scale);
    xmm_out = _mm_mul_ss(xmm_in, xmm_scale);
    _mm_store_ss(&out[2], xmm_out);
#else
    out[0] = in[0]*scale;
    out[1] = in[1]*scale;
    out[2] = in[2]*scale;
#endif
}

void VectorConstruct(float vec1, float vec2, float vec3, vec3_t out)
{
#if __SSE__
    // raynorpat: sse optimization
    __m128 xmm_in;

    xmm_in = _mm_load_ss(&vec1);
    _mm_store_ss(&out[0], xmm_in);

    xmm_in = _mm_load_ss(&vec2);
    _mm_store_ss(&out[1], xmm_in);

    xmm_in = _mm_load_ss(&vec3);
    _mm_store_ss(&out[2], xmm_in);
#else
    out[0] = vec1;
    out[1] = vec2;
    out[2] = vec3;
#endif
}

/*-----------------------------------------------------------------*/

float	anglemod(float a)
{
    a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
    return a;
}

float angledelta (float a)
{
    a = anglemod(a);

    if (a > 180)
    {
        a -= 360;
    }
    return a;
}

/*
==================
BOPS_Error

Split out like this for ASM to call.
==================
*/
void BOPS_Error (void)
{
    Sys_Error ("BoxOnPlaneSide:  Bad signbits");
}

/*
==================
BoxOnPlaneSide

Returns 1, 2, or 1 + 2
==================
*/
int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
{
    float	dist1, dist2;
    int		sides;

// general case
    switch (p->signbits)
    {
    case 0:
        dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
        dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
        break;
    case 1:
        dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
        dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
        break;
    case 2:
        dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
        dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
        break;
    case 3:
        dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
        dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
        break;
    case 4:
        dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
        dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
        break;
    case 5:
        dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
        dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
        break;
    case 6:
        dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
        dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
        break;
    case 7:
        dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
        dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
        break;
    default:
        dist1 = dist2 = 0;		// shut up compiler
        BOPS_Error ();
        break;
    }
    sides = 0;

    if (dist1 >= p->dist)
    {
        sides = 1;
    }

    if (dist2 < p->dist)
    {
        sides |= 2;
    }

#ifdef PARANOID
    if (sides == 0)
    {
        Sys_Error ("BoxOnPlaneSide: sides == 0");
    }
#endif
    return sides;
}

void VectorAngles (const vec3_t normal, vec3_t angles)
{
    float forward, yaw, pitch;

    if (!normal[1] && !normal[0])
    {
        yaw = 0;
        pitch = (normal[2] > 0) ? 90 : 270;
    }
    else
    {
        yaw = normal[0] ? (atan2(normal[1], normal[0]) * 180 / M_PI) : (normal[1] > 0) ? 90 : 270;

        if (yaw < 0)
        {
            yaw += 360;
        }
        forward = sqrt (normal[0] * normal[0] + normal[1] * normal[1]);

        pitch = atan2 (normal[2], forward) * 180 / M_PI;

        if (pitch < 0)
        {
            pitch += 360;
        }
    }
    angles[0] = pitch;
    angles[1] = yaw;
    angles[2] = 0;
}

void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
    float		angle;
    float		sr, sp, sy, cr, cp, cy;

    angle = angles[YAW] * (M_PI*2 / 360);
    sy = sin(angle);
    cy = cos(angle);

    angle = angles[PITCH] * (M_PI*2 / 360);
    sp = sin(angle);
    cp = cos(angle);

    angle = angles[ROLL] * (M_PI*2 / 360);
    sr = sin(angle);
    cr = cos(angle);

    forward[0] = cp*cy;
    forward[1] = cp*sy;
    forward[2] = -sp;

    right[0] = (-1*sr*sp*cy+-1*cr*-sy);
    right[1] = (-1*sr*sp*sy+-1*cr*cy);
    right[2] = -1*sr*cp;

    up[0] = (cr*sp*cy+-sr*-sy);
    up[1] = (cr*sp*sy+-sr*cy);
    up[2] = cr*cp;
}

float Length(vec3_t inout)
{
    int		i;
    float	length;

    length = 0;

    for (i=0 ; i< 3 ; i++)
    {
        length += inout[i] * inout[i];
    }
    length = sqrt((double)length);		// FIXME

    return length;
}

float Distance(const vec3_t v, const vec3_t v2)
{
    int		i;
    float	length, d;

    length = 0;

    for (i=0 ; i< 3 ; i++)
    {
        d = v[i] - v2[i];
        length += d*d;
    }
    length = sqrt ((double)length);		// FIXME

    return length;
}

float Normalize (vec3_t inout)
{
    float	length, ilength;

    length = sqrt ((double)inout[0] * inout[0] + inout[1] * inout[1] + inout[2] * inout[2]);

    if (!length)
    {
        VectorClear (inout);
        return 0;
    }
    ilength = 1.0 / length;

    inout[0] = inout[0]*ilength;
    inout[1] = inout[1]*ilength;
    inout[2] = inout[2]*ilength;

    return length;
}

void Mat_Mul_1x4_4x4(matrix_1x4 a, matrix_4x4 b, matrix_1x4 result)
{
// this function multiplies a 1x4 by a 4x4 and stores the result in a 1x4

    int index_j,    // column index
        index_k;    // row index

    float sum;    // temp used to hold sum of products

    // loop thru columns of b
    for (index_j=0; index_j<4; index_j++)
    {

        // multiply ith row of a by jth column of b and store the sum
        // of products in the position i,j of result
        sum=0;

        for (index_k=0; index_k<4; index_k++)
        {
            sum+=a[index_k]*b[index_k][index_j];
        }

        // store result
        result[index_j] = sum;

    } // end for index_j

} // end Mat_Mul_1x4_4x4

/*
PENTA: Easy & fast matrix inversions with some quirks (just what Carmack likes ;) )
	Thnx to http://www.cs.unc.edu/~gotz/code/affinverse.html
	Find the inverse of a matrix that is made up of only scales, rotations,
	and translations.
*/
void MatrixAffineInverse( matrix_4x4  m, matrix_4x4  result )
{
    float Tx, Ty, Tz;

    // The rotational part of the matrix is simply the transpose of the
    // original matrix.
    result[0][0] = m[0][0];
    result[1][0] = m[0][1];
    result[2][0] = m[0][2];

    result[0][1] = m[1][0];
    result[1][1] = m[1][1];
    result[2][1] = m[1][2];

    result[0][2] = m[2][0];
    result[1][2] = m[2][1];
    result[2][2] = m[2][2];

    // The right column vector of the matrix should always be [ 0 0 0 1 ]
    // In most cases. . . you don't need this column at all because it'll
    // never be used in the program, but since this code is used with GL
    // and it does consider this column, it is here.
    result[0][3] = result[1][3] = result[2][3] = 0;
    result[3][3] = 1;

    // The translation components of the original matrix.
    Tx = m[3][0];
    Ty = m[3][1];
    Tz = m[3][2];

    // Rresult = -(Tm * Rm) to get the translation part of the inverse
    result[3][0] = -( m[0][0] * Tx + m[0][1] * Ty + m[0][2] * Tz );
    result[3][1] = -( m[1][0] * Tx + m[1][1] * Ty + m[1][2] * Tz );
    result[3][2] = -( m[2][0] * Tx + m[2][1] * Ty + m[2][2] * Tz );
}
